
#include "lwip/sys.h"
#include "rtps/discovery/ParticipantProxyData.h"
#include "rtps/entities/Participant.h"
#include "rtps/entities/Reader.h"
#include "rtps/entities/Writer.h"
#include "rtps/entities/StatelessReader.h"
#include "rtps/messages/MessageTypes.h"
#include "rtps/utils/Log.h"
#include "rtps/utils/udpUtils.h"
#include "rtps/discovery/SPDPAgent.h"

#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE
#include "rtps/utils/printutils.h"
#define SPDP_LOG(...)                                                          \
  if (true) {                                                                  \
    printf("[SPDP] ");                                                         \
    printf(__VA_ARGS__);                                                       \
    printf("\n");                                                              \
  }
#else
#define SPDP_LOG(...) //
#endif

void rtps_SPDPAgent_destruct(struct rtps_SPDPAgent *this) {
    if(this->initialized) {
        sys_mutex_free(&this->m_mutex);
    }
}

void rtps_SPDPAgent_init(struct rtps_SPDPAgent *this, struct rtps_Participant *participant, struct rtps_BuiltInEndpoints *endpoints) {
    if (sys_mutex_new(&this->m_mutex) != ERR_OK) {
        SPDP_LOG("Could not alloc mutex");
        return;
    }
    this->mp_participant = participant;
    this->m_buildInEndpoints.spdpWriter = endpoints->spdpWriter;
    this->m_buildInEndpoints.spdpReader = endpoints->spdpReader;
    this->m_buildInEndpoints.sedpPubWriter = endpoints->sedpPubWriter;
    this->m_buildInEndpoints.sedpPubReader = endpoints->sedpPubReader;
    this->m_buildInEndpoints.sedpSubWriter = endpoints->sedpSubWriter;
    this->m_buildInEndpoints.sedpSubReader = endpoints->sedpSubReader;
    rtps_StatelessReader_registerCallback(this->m_buildInEndpoints.spdpReader, rtps_SPDPAgent_receiveCallback, this);
    
    ucdr_init_buffer(&(this->m_microbuffer), this->m_outputBuffer, sizeof(this->m_outputBuffer) / sizeof(this->m_outputBuffer[0]));
    // addInlineQos();
    rtps_SPDPAgent_addParticipantParameters(this);
    rtps_ParticipantProxyData_Init_Default(&this->m_proxyDataBuffer);
    this->initialized = true;
}

void rtps_SPDPAgent_start(struct rtps_SPDPAgent *this) {
    if (this->m_running) {
        return;
    }
    this->m_running = true;
    sys_thread_new("SPDPThread", rtps_SPDPAgent_runBroadcast, this, SPDP_WRITER_STACKSIZE, SPDP_WRITER_PRIO);
}

void rtps_SPDPAgent_stop(struct rtps_SPDPAgent *this) {
    this->m_running = false;
}

void rtps_SPDPAgent_runBroadcast(void *args) {
    struct rtps_SPDPAgent *agent = (struct rtps_SPDPAgent*)args;
    const DataSize_t size = ucdr_buffer_length(&(agent->m_microbuffer));

    rtps_StatelessWriter_newChange(agent->m_buildInEndpoints.spdpWriter, ALIVE, agent->m_microbuffer.init, size);

    while (agent->m_running) {
#ifdef OS_IS_FREERTOS
        vTaskDelay(pdMS_TO_TICKS(Config::SPDP_RESEND_PERIOD_MS));
#else
        sys_msleep(SPDP_RESEND_PERIOD_MS);
#endif
        rtps_StatelessWriter_setAllChangesToUnsent(agent->m_buildInEndpoints.spdpWriter);        
        if (agent->m_cycleHB >= SPDP_CYCLECOUNT_HEARTBEAT) {
            agent->m_cycleHB = 0;
            rtps_Participant_checkAndResetHeartbeats(agent->mp_participant);
        } else {
            agent->m_cycleHB++;
        }
    } 
}

void rtps_SPDPAgent_receiveCallback(void *callee, const struct rtps_ReaderCacheChange *cacheChange) {
    struct rtps_SPDPAgent *agent = (struct rtps_SPDPAgent*)callee;
    SPDP_LOG("receive SPDP package.\n");
    rtps_SPDPAgent_handleSPDPPackage(agent, cacheChange);
}

void rtps_SPDPAgent_handleSPDPPackage(struct rtps_SPDPAgent *this, const struct rtps_ReaderCacheChange *cacheChange) {
    if (!this->initialized) {
        SPDP_LOG("Callback called without initialization\n");
        return;
    }
    rtps_Lock(&this->m_mutex);
    if (cacheChange->size > sizeof(this->m_inputBuffer) / sizeof(this->m_inputBuffer[0])) {
        SPDP_LOG("Input buffer to small\n");
        rtps_Unlock(&this->m_mutex);
        return;
    }
    // Something went wrong deserializing remote participant
    if(!rtps_ReaderCacheChange_copyInto(cacheChange, this->m_inputBuffer, sizeof(this->m_inputBuffer) / sizeof(this->m_inputBuffer[0]))){
        rtps_Unlock(&this->m_mutex);
        return;
    }

    struct ucdrBuffer buffer;
    ucdr_init_buffer(&buffer, this->m_inputBuffer, sizeof(this->m_inputBuffer) / sizeof(this->m_inputBuffer[0]));

    if (cacheChange->kind == ALIVE) {
        rtps_SPDPAgent_configureEndianessAndOptions(&buffer);
        bool success =  rtps_ParticipantProxyData_readFromUcdrBuffer(&(this->m_proxyDataBuffer), &buffer);
        if (success) {
            // TODO In case we store the history we can free the history mutex here
            rtps_SPDPAgent_processProxyData(this);
        }
    } else {
    // TODO RemoveParticipant
    }

    rtps_Unlock(&this->m_mutex);
}

void rtps_SPDPAgent_configureEndianessAndOptions(ucdrBuffer *buffer) {
    uint8_t encapsulation[2]; 
    // Endianess doesn't matter for this since those are single bytes
    ucdr_deserialize_array_uint8_t(buffer, encapsulation, sizeof(encapsulation) / sizeof(encapsulation[0]));
    
    int flag = 0;
    int len = sizeof(encapsulation) / sizeof(encapsulation[0]);
    for(int i = 0; i < len; i++) {
        if(encapsulation[i] != SCHEME_PL_CDR_LE[i]) {
            flag = 1;
            break;
        }
    }
    
    if (flag == 0) {
        buffer->endianness = UCDR_LITTLE_ENDIANNESS;
    } else {
        buffer->endianness = UCDR_BIG_ENDIANNESS;
    }
    // Reuse encapsulation buffer to skip options
    ucdr_deserialize_array_uint8_t(buffer, encapsulation, sizeof(encapsulation) / sizeof(encapsulation[0]));
}

void rtps_SPDPAgent_processProxyData(struct rtps_SPDPAgent *this) {
    if (this->m_proxyDataBuffer.m_guid.prefix.id == this->mp_participant->m_guidPrefix.id) {
        return; // Our own packet
    }

    struct rtps_ParticipantProxyData *remote_part;

    remote_part = rtps_Participant_findRemoteParticipant(this->mp_participant, &(this->m_proxyDataBuffer.m_guid.prefix));
    if (remote_part != NULL) {
        SPDP_LOG("Not adding remote participant guid.prefix = %u \n", (unsigned int)rtps_Guid_t_operator_sum(&remote_part->m_guid.prefix));
        rtps_Participant_refreshRemoteParticipantLiveliness(this->mp_participant, &(this->m_proxyDataBuffer.m_guid.prefix));
        rtps_Participant_findRemoteParticipant_free(remote_part);
        return; // Already in our list
    }
    int ret = rtps_Participant_addNewRemoteParticipant(this->mp_participant, &(this->m_proxyDataBuffer));
    if (ret) {
        rtps_SPDPAgent_addProxiesForBuiltInEndpoints(this);
        rtps_StatelessWriter_setAllChangesToUnsent(this->m_buildInEndpoints.spdpWriter);
        
#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE
        SPDP_LOG("Added new participant with guid: ");
        printGuidPrefix(this->m_proxyDataBuffer.m_guid.prefix);
#endif
    } else{
        SPDP_LOG("Failed to add new participant");
    }
    rtps_Participant_findRemoteParticipant_free(remote_part);
}

bool rtps_SPDPAgent_addProxiesForBuiltInEndpoints(struct rtps_SPDPAgent *this) {

    struct Locator *locator = NULL;
    // Check if the remote participants has a locator in our subnet
    for (unsigned int i = 0; i < 5; i++) {
        struct Locator *local = &(this->m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]);
        if(rtps_Locator_isValid(local) && rtps_Locator_isSameSubnet(local)) {
            locator = local;
            break;
        }
    }

    if (!locator) {
        return false;
    }
#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE
    ip4_addr_t ip4addr = rtps_Locator_getIp4Address(locator);
    const char *addr = ip4addr_ntoa(&ip4addr);
    SPDP_LOG("Adding IPv4 Locator %s\n", addr);
#endif
    if (rtps_ParticipantProxyData_hasPublicationWriter(&(this->m_proxyDataBuffer))) {
        struct Guid_t guid_temp;
        struct rtps_WriterProxy proxy;
        guid_temp.prefix = this->m_proxyDataBuffer.m_guid.prefix;
        guid_temp.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER;
        rtps_WriterProxy_construct(&proxy, &guid_temp, locator);
        rtps_StatefulReader_addNewMatchedWriter(this->m_buildInEndpoints.sedpPubReader, &proxy);
    }

    if (rtps_ParticipantProxyData_hasSubscriptionWriter(&(this->m_proxyDataBuffer))) {
        struct Guid_t guid_temp;
        struct rtps_WriterProxy proxy;
        guid_temp.prefix = this->m_proxyDataBuffer.m_guid.prefix;
        guid_temp.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER;
        rtps_WriterProxy_construct(&proxy, &guid_temp, locator);
        rtps_StatefulReader_addNewMatchedWriter(this->m_buildInEndpoints.sedpSubReader, &proxy);
    }

    if (rtps_ParticipantProxyData_hasPublicationReader(&(this->m_proxyDataBuffer))) {
        struct Guid_t guid_temp;
        struct rtps_ReaderProxy proxy;
        guid_temp.prefix = this->m_proxyDataBuffer.m_guid.prefix;
        guid_temp.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER;
        rtps_ReaderProxy_Construct(&proxy, &guid_temp, locator, NULL);
        rtps_StatefulWriter_addNewMatchedReader(this->m_buildInEndpoints.sedpPubWriter, (const ReaderProxy *)&proxy);
    }

    if (rtps_ParticipantProxyData_hasSubscriptionReader(&(this->m_proxyDataBuffer))) {
        struct Guid_t guid_temp;
        struct rtps_ReaderProxy proxy;
        guid_temp.prefix = this->m_proxyDataBuffer.m_guid.prefix;
        guid_temp.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER;
        rtps_ReaderProxy_Construct(&proxy, &guid_temp, locator, NULL);
        rtps_StatefulWriter_addNewMatchedReader(this->m_buildInEndpoints.sedpSubWriter, (const ReaderProxy *)&proxy);
    }

    return true;
}
extern const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT;

void rtps_SPDPAgent_addInlineQos(struct rtps_SPDPAgent *this) {
    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_KEY_HASH);
    ucdr_serialize_uint16_t(&this->m_microbuffer, 16);
    GuidPrefix_t guidPrefix_temp; 
    ucdr_serialize_array_uint8_t(&this->m_microbuffer,
                               (this->mp_participant->m_guidPrefix).id,
                               sizeof(guidPrefix_temp.id));
    EntityId_t entityId_temp;
    ucdr_serialize_array_uint8_t(&this->m_microbuffer,
                               ENTITYID_BUILD_IN_PARTICIPANT.entityKey,
                               sizeof(entityId_temp.entityKey));
    ucdr_serialize_uint8_t(&this->m_microbuffer, (uint8_t)(ENTITYID_BUILD_IN_PARTICIPANT.entityKind));

    rtps_SPDPAgent_endCurrentList(this);
}

void rtps_SPDPAgent_endCurrentList(struct rtps_SPDPAgent *this) {
    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_SENTINEL);
    ucdr_serialize_uint16_t(&this->m_microbuffer, 0);
}

void rtps_SPDPAgent_addParticipantParameters(struct rtps_SPDPAgent *this) {
    const uint16_t zero_options = 0;
    const uint16_t protocolVersionSize = sizeof(PROTOCOLVERSION.major) + sizeof(PROTOCOLVERSION.minor);
    const uint16_t vendorIdSize = 2;
    const uint16_t locatorSize = sizeof(Locator);
    const uint16_t durationSize = 4 + 4;
    const uint16_t entityKeySize = 3;
    const uint16_t entityKindSize = 1;
    const uint16_t entityIdSize = entityKeySize + entityKindSize;
    const uint16_t guidSize = 12 + entityIdSize;

    Locator userUniCastLocator;
    Locator builtInUniCastLocator;
    Locator builtInMultiCastLocator;
    rtps_Locator_getUserUnicastLocator(&userUniCastLocator, this->mp_participant->m_participantId);
    rtps_Locator_getBuiltInUnicastLocator(&builtInUniCastLocator, this->mp_participant->m_participantId);
    rtps_Locator_getBuiltInMulticastLocator(&builtInMultiCastLocator);

    ucdr_serialize_array_uint8_t(&this->m_microbuffer, SCHEME_PL_CDR_LE, sizeof(SCHEME_PL_CDR_LE)/sizeof(SCHEME_PL_CDR_LE[0]));
    ucdr_serialize_uint16_t(&this->m_microbuffer, zero_options);

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_PROTOCOL_VERSION);
    ucdr_serialize_uint16_t(&this->m_microbuffer, protocolVersionSize + 2);
    ucdr_serialize_uint8_t(&this->m_microbuffer, PROTOCOLVERSION.major);
    ucdr_serialize_uint8_t(&this->m_microbuffer, PROTOCOLVERSION.minor);
    (this->m_microbuffer).iterator += 2;      // padding
    (this->m_microbuffer).last_data_size = 4; // to 4 byte

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_VENDORID);
    ucdr_serialize_uint16_t(&this->m_microbuffer, vendorIdSize + 2);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, VENDOR_ID.vendorId, vendorIdSize);
    this->m_microbuffer.iterator += 2;      // padding
    this->m_microbuffer.last_data_size = 4; // to 4 byte

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_DEFAULT_UNICAST_LOCATOR);
    ucdr_serialize_uint16_t(&this->m_microbuffer, locatorSize);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, (const uint8_t *)(&userUniCastLocator), locatorSize);

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_METATRAFFIC_UNICAST_LOCATOR);
    ucdr_serialize_uint16_t(&this->m_microbuffer, locatorSize);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, (const uint8_t *)(&builtInUniCastLocator), locatorSize);

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_METATRAFFIC_MULTICAST_LOCATOR);
    ucdr_serialize_uint16_t(&this->m_microbuffer, locatorSize);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, (const uint8_t *)(&builtInMultiCastLocator), locatorSize);

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_PARTICIPANT_LEASE_DURATION);
    ucdr_serialize_uint16_t(&this->m_microbuffer, durationSize);
    ucdr_serialize_int32_t(&this->m_microbuffer, SPDP_DEFAULT_REMOTE_LEASE_DURATION.seconds);
    ucdr_serialize_uint32_t(&this->m_microbuffer, SPDP_DEFAULT_REMOTE_LEASE_DURATION.fraction);

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_PARTICIPANT_GUID);
    ucdr_serialize_uint16_t(&this->m_microbuffer, guidSize);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, this->mp_participant->m_guidPrefix.id, 12);
    ucdr_serialize_array_uint8_t(&this->m_microbuffer, ENTITYID_BUILD_IN_PARTICIPANT.entityKey, entityKeySize);
    ucdr_serialize_uint8_t(&this->m_microbuffer, (uint8_t)(ENTITYID_BUILD_IN_PARTICIPANT.entityKind));

    ucdr_serialize_uint16_t(&this->m_microbuffer, PID_BUILTIN_ENDPOINT_SET);
    ucdr_serialize_uint16_t(&this->m_microbuffer, sizeof(BuildInEndpointSet));
    ucdr_serialize_uint32_t(&this->m_microbuffer, DISC_BIE_PARTICIPANT_ANNOUNCER |
                            DISC_BIE_PARTICIPANT_DETECTOR |
                            DISC_BIE_PUBLICATION_ANNOUNCER |
                            DISC_BIE_PUBLICATION_DETECTOR |
                            DISC_BIE_SUBSCRIPTION_ANNOUNCER |
                            DISC_BIE_SUBSCRIPTION_DETECTOR);

    rtps_SPDPAgent_endCurrentList(this);
}

#undef SPDP_VERBOSE
